#ifndef ZUCPOS_H
#define ZUCPOS_H

#include "../../libnml/posemath/posemath.h" /* PmCartesian */
#include "modbus/mb_slave.h"
#include "pnio_mem.h"
#include "eipio_mem.h"
#include "rtdev/tio_gripper.h"
#include <sys/select.h>
#include "zuc/motion/zucmotcfg.h"
#ifdef __cplusplus
#include "zuc/nml_intf/zucpos2.h"
#include "safety/io/safeiotype.h"
#else
typedef struct ZucPose
{
    PmCartesian tran;
    double a, b, c;
    double u, v, w;
} ZucPose;
#endif

typedef unsigned long int KINEMATICS_FORWARD_FLAGS;
typedef unsigned long int KINEMATICS_INVERSE_FLAGS;

#define NUM_JOINTS 6
#define MAX_TOOL_CNT 16
#define MAX_FRAME_CNT 16
#define MAX_PAYLOAD_CNT 16
#define CART_DIM 6

// TIO传感器品牌号
#define TIO_SENSOR_BRAND 6

#define ERROR_OFFSET_FOR_JOINT_NUM 16

#define MAX_TIO_SIG_NUM 8

#define MAX_EXTIO_CNT 8
#define MAX_EXTIO_DI_NUM 64
#define MAX_EXTIO_DO_NUM 64
#define MAX_EXTIO_AI_NUM 32
#define MAX_EXTIO_AO_NUM 32
#define MAX_EXTIO_NAME_LEN 129  // '\0'

#define RUN_MODE_UNSPECIFIED -1
#define RUN_MODE_REAL 0
#define RUN_MODE_REAL_NO_CAN_CHECK 1
#define RUN_MODE_SIM_WITHOUT_CAN 2
#define RUN_MODE_SIM_ON_REAL 3

#define MODEL_UNSUPPORTED 65535
#define MODEL_ZU_310 310
#define MODEL_ZU_311 311
#define MODEL_ZU_312 312
#define MODEL_ZU_313 313
#define MODEL_C_510 510
#define MODEL_C_511 511
#define MODEL_ZU_512 512
#define MODEL_ZU_513 513
#define MODEL_ZU_514 514
#define MODEL_PRO_515 515
#define MODEL_ZU_710 710
#define MODEL_ZU_720 720
#define MODEL_ZU_721 721
#define MODEL_ZU_722 722
#define MODEL_ZU_723 723
#define MODEL_ZU_724 724
#define MODEL_ZU_725 725
#define MODEL_ZU_726 726
#define MODEL_ZU_727 727
#define MODEL_ZU_728 728
#define MODEL_C_710 729
#define MODEL_C_711 730
#define MODEL_PRO_731 731
#define MODEL_ZU_1210 1210
#define MODEL_ZU_1211 1211
#define MODEL_ZU_1212 1212
#define MODEL_PRO_1213 1213
#define MODEL_PRO_1610 1610
#define MODEL_ZU_1810 1810
#define MODEL_ZU_1811 1811
#define MODEL_ZU_1812 1812
#define MODEL_PRO_1813 1813
#define MODEL_ZU_2010 2010
#define MODEL_ZU_2020 2020

#define MODEL_ZU_2011 2011
#define MODEL_ZU_2012 2012
#define MODEL_CO_0115 10115
#define MODEL_CO_1115 11115
#define MODEL_CO_1315 11315
#define MODEL_CO_1415 11415
#define MODEL_CO_2015 12015
#define MODEL_CO_2120 12120

// 7轴,阿童木
#define MODEL_AT_0710 22023

// 双臂
#define MODEL_DU_2001_L 22001  // 给ZY的早期版本
#define MODEL_DU_2002_R 22002  // 给ZY的早期版本
#define MODEL_DU_311_L 20311
#define MODEL_DU_312_R 20312

// MAX
#define MODEL_MAX_3510 33510

#define T_PLANNER 0
#define S_PLANNER 1
#define TOPP_PLANNER 2

typedef enum
{
    TC_NONE = 0,
    TC_LINEAR = 1,
    TC_CIRCULAR = 2,
    TC_RIGIDTAP = 3,
    TC_SPHERICAL = 4,
    TC_JOINT = 5,
    TC_MOVC = 6,
    TC_CUBICPOLY = 7,  //WH 添加,
    TC_JOINTBLEND = 8,
    TC_MODE_FREE = 9,
    TC_MOVS = 10,
    TC_ESTOP = 11
} tc_motion_type_t;

#define TP_ACCEL_EPSILON 1e-4
#define TP_VEL_EPSILON 1e-8
#define TP_POS_EPSILON 1e-5
#define TP_TIME_EPSILON 1e-12
#define TP_ANGLE_EPSILON 1e-6
#define TP_MIN_ARC_ANGLE 1e-3
#define TP_MIN_ARC_LENGTH 1e-6
#define TP_BIG_NUM 1e10

typedef enum
{
    IODevTYPE_PHYSIC = 0,
    IODevTYPE_TIO = 1,
    IODevTYPE_EXTIO = 2,
    IODevTYPE_CAB_INTERNAL = 3,
    IODevTYPE_SYNCH_MB_SLAVE = 4,
    IODevTYPE_SYNCH_PN_DEV = 5,
    IODevTYPE_SYNCH_EIP_ADPT = 6
} IODevType;

/* values for endFlag */
typedef enum
{
    TC_TERM_COND_STOP = 0,       //停止
    TC_TERM_COND_EXACT = 1,      //精确
    TC_TERM_COND_PARABOLIC = 2,  //抛物线
    TC_TERM_COND_TANGENT = 3     //切线
} tc_term_cond_t;

typedef enum
{
    ADIO_INPUT = 0,
    ADIO_OUTPUT,
    ADIO_JOINT,
    ADIO_TCP,
    ADIO_BASE,
    ADIO_TOOL,
    ADIO_UHI,  // user high interface
    ADIO_ROBOT_OUT,
    ADIO_SAFETY_OUT
} IOtype;

typedef enum
{
    ADIO_UINT8 = 0,
    ADIO_INT16,
    ADIO_INT32,
    ADIO_FLOAT
} IODataType;

typedef union
{
    float IO_FLT;
    int32_t IO_int32;
    uint8_t IO_uint8[4];
    int16_t IO_int16[2];
} IO_float;

/**
 * TP return codes.
 * This enum is a catch-all for useful return statuses from TP
 * internal functions. This may be replaced with a better system in
 * the future.
 */
typedef enum
{
    TP_ERR_JOINT_VEL_ZERO = -10,
    TP_ERR_INVALID = -9,
    TP_ERR_INPUT_TYPE = -8,
    TP_ERR_TOLERANCE = -7,
    TP_ERR_RADIUS_TOO_SMALL = -6,
    TP_ERR_GEOM = -5,
    TP_ERR_RANGE = -4,
    TP_ERR_MISSING_OUTPUT = -3,
    TP_ERR_MISSING_INPUT = -2,
    TP_ERR_FAIL = -1,
    TP_ERR_OK = 0,
    TP_ERR_NO_ACTION,
    TP_ERR_SLOWING,
    TP_ERR_STOPPED,
    TP_ERR_WAITING,
    TP_ERR_ZERO_LENGTH,
    TP_ERR_LAST,
    TP_ERR_KINE,
    TP_ERR_COLLISION_REBOUND
} tp_err_t;

typedef enum
{
    NO_ERROR = 0,
    FAILED_FIND_FEASIBLE_SETS,
    CROSS_SAFETY_ZONE
} tp_toppra_err_t;

typedef struct ZucJPose
{
    double jpos[ZUCMOT_MAX_JOINTS];
} ZucJPose;

typedef struct EndCond
{
    int di_type;
    int di_index;
    short di_stat;
} EndCond;
typedef struct
{
    PmHomogeneous start;
    PmHomogeneous end;
    PmCartesian center;
    PmCartesian normal;
    double radius;
    double angle;
    int turn;
    PmRotationVector abc;
    int using_abc;
    double count;
} MovcStruct;

typedef struct ConstraintsToppra
{
    double vel_lim[6][2];
    double acc_lim[6][2];
    double tor_lim[6][2];
    double req_vel[2];  //关节空间或笛卡尔空间的最大指令速度,暂时先放这个结构体中; req_vel[0] = 0: movj; req_vel[0] = 1: movl; req_vel[1]: 指令速度
    int lim_add[3];   //1th: 1:add vel limit; 2th: 1:add acc limit  ---->  changed as filter size; 3th: 1:add tor limit
} ConstraintsToppra;  //给定的关节约束(最大/小关节速度、最大/小关节加速度、最大/小关节力矩)

typedef struct PathCoefs
{
    double pathq_coefs[6][4];
} PathCoefs;

// typedef struct
// {
//     double pathq_coefs[6][4];
//     // double pathvel_coefs[6][3];//路径一阶导系数
//     // double pathacc_coefs[6][2];//路径二阶导系数
//     double ss;
//     double ss_seg_start;  // the segment s tart point
//     double min_time;      //cal by reqvel, min_time changes with scale
//     // double pathq_coefs_next[6][4];
//     // double pathvel_coefs_next[6][3];//路径一阶导系数
//     // double pathacc_coefs_next[6][2];//路径二阶导系数
//     // double ss_next;// next指的是“后-->前”方向的next
//     // double ss_seg_start_next;// the segment start point

//     // double jointpos[6];
//     // double min_time;// by req_vel
//     // double x_by_reqvel;
//     // double reqvel[2];// reqvel[0]->cart(0) or joint vel(1); reqvel[1]->vel value

//     double jtor_a[12];  // torque constraints: 12 = 2 * dof, interpolation discretization scheme considers the current and next gridpoints;
//     double jtor_b[12];
//     double jtor_c[12];
//     double jtor_d[12];

//     double jacc_a[12];  // joint acc constraints: 12 = 2 * dof, interpolation discretization scheme considers the current and next gridpoints;
//     double jacc_b[12];

//     double xbound[2];  // joint vel constraints: xbound[0]: min; xbound[1]: max
//     // double control_set[2];
//     // double optimal_u;// optimal path acceleration
//     // double reachable_set;

//     // int constraints_num;
//     // double b_28[28];
//     // double A_56[56];

//     // double b_52[52];
//     // double A_104[104];
//     // double b[52];
//     // double A[104];

//     // double ave_path_vel;// between i and i - 1
//     // double grid_time;// between i and i - 1
//     // double total_time;// 运动至该网格点所用的总时间，用于计算关节位置

// } PathInfo;  //每段路径拟合函数对应系数(六个关节)

typedef struct
{
    int tc_id;
    int finish_fw;
    int finish_pause_rset;
    double pathq_coefs[6][4];
    // double pathvel_coefs[6][3];//路径一阶导系数
    // double pathacc_coefs[6][2];//路径二阶导系数
    double ss;
    double ss_seg_start;  // the segment s tart point
    double min_time;

    // double pathq_coefs_next[6][4];
    // double pathvel_coefs_next[6][3];//路径一阶导系数
    // double pathacc_coefs_next[6][2];//路径二阶导系数
    // double ss_next;// next指的是“后-->前”方向的next
    // double ss_seg_start_next;// the segment start point

    // double jointpos[6];
    // double min_time;// by req_vel
    // double x_by_reqvel;
    // double reqvel[2];// reqvel[0]->cart(0) or joint vel(1); reqvel[1]->vel value

    double jtor_a[2 *
                  ZUCMOT_ROBOT_MAX_JOINTS];  // torque constraints: 12 = 2 * dof, interpolation discretization scheme considers the current and next gridpoints;
    double jtor_b[2 * ZUCMOT_ROBOT_MAX_JOINTS];
    double jtor_c[2 * ZUCMOT_ROBOT_MAX_JOINTS];
    double jtor_d[2 * ZUCMOT_ROBOT_MAX_JOINTS];

    // double jacc_a[12];  // joint acc constraints: 12 = 2 * dof, interpolation discretization scheme considers the current and next gridpoints;
    // double jacc_b[12];

    double xbound[2];       // joint vel constraints: xbound[0]: min; xbound[1]: max
    double control_set[2];  //[0]:lower; [1]:upper
    double optimal_u;       // optimal path acceleration
    double reachable_set;

    int constraints_num;
    int constraints_tor_num;
    int constraints_acc_num;
    // double b_28[28];
    // double A_56[56];

    // double b_52[52];
    // double A_104[104];
    double b[28];
    double A[56];

    double ave_path_vel;  // between i and i - 1
    double grid_time;     // between i and i - 1
    double total_time;    // 运动至该网格点所用的总时间，用于计算关节位置

} TcPathInfo;  //每段路径拟合函数对应系数(六个关节)

typedef struct
{
    TcPathInfo* path_infom; /* ptr to the tcs */
    int size;               /* size of queue */
    int _len;               /* number of tcs now in queue */
    int front, rear;        /* indices to next to get, next to put */
} PathInfoQueue;

typedef struct
{
    double blendjpos[ZUCMOT_MAX_JOINTS]; /* ptr to the tcs */
} BlendJpos;

typedef struct
{
    BlendJpos* jpos; /* ptr to the tcs */
    int size;        /* size of queue */
    int _len;        /* number of tcs now in queue */
    int front, rear; /* indices to next to get, next to put */
} BlendSegPath;

typedef struct
{
    PmHomogeneous T1_to_world;  //第一关节坐标在世界坐标系下的齐次矩阵
    PmHomogeneous T2_to_world;
    PmHomogeneous T3_to_world;
} MultiPmHomogeneous_elbow;

typedef struct DHParam
{
    float alpha[ZUCMOT_MAX_JOINTS];
    float a[ZUCMOT_MAX_JOINTS];
    float d[ZUCMOT_MAX_JOINTS];
    float joint_homeoff[ZUCMOT_MAX_JOINTS];
} DHParam;

//存放机器人关节限位、速度上限、加速度上限结构体
typedef struct Robot_joint_Pos_Limit
{
    double Joint_Pos_Up[7];
    double Joint_Pos_low[7];
} Robot_joint_Pos_Limit;

typedef struct Robot_joint_Vel_Limit
{
    double Joint_Vel_Up[7];
    //double Joint_Vel_low[Robot_DOF];
} Robot_joint_Vel_Limit;

typedef struct Robot_joint_Acc_Limit
{
    double Joint_Acc_Up[7];
    //double Joint_Acc_low[Robot_DOF];
} Robot_joint_Acc_Limit;

typedef struct
{
    Robot_joint_Pos_Limit Joint_Pos_Limit;
    Robot_joint_Vel_Limit Joint_Vel_Limit;
    Robot_joint_Acc_Limit Joint_Acc_Limit;
} Robot_joint_Limit;

//multi_obj 参数最大速度、最大加速度倍率、加速度目标函数选择参数、delta_t
typedef struct
{
    double V_coe;
    double A_coe;
    double s_a;
    double delta_t;
} multi_obj_Algo_param;

//臂角可行区间链表
typedef struct Arm_angle_inte
{
    double Arm_a;
    int Flah_L_U;
    struct Arm_angle_inte* next;
} Arm_angle_inte;
typedef struct Arm_angle_inte* Arm_a_List_Ptr;

typedef struct
{
    double k;
    double alpha;
} joint_lim_Algo_param;

typedef struct Payload
{
    double m;     ///<mass
    double p[3];  ///<center of mass
} Payload;

typedef struct Dynamics
{
    double ele_dynamics[88];
    double friction_dynamics[88];
    double max_err[7];
    double mean_err[7];
    double std_err[7];
} Dynamics;

typedef struct CollisionOption
{
    int collisionMethod;
    int reactionType;
    float reboundAngle;
    float reboundLength;
    float rebound_j_acc;
    float rebound_j_vel;
    float rebound_l_acc;
    float rebound_l_vel;
} CollisionOption;

typedef struct Quaternion
{
    double s;
    double x;
    double y;
    double z;
} Quaternion;

typedef struct RPY
{
    double r;
    double p;
    double y;
} RPY;

typedef struct BaseOffset
{
    Quaternion quat;
    double appang[3];
} BaseOffset;

typedef struct IO
{
    short type;  // io type: 0-->basic io, 1-->tio; 2-->modbus io; 3-->tcp io
    short index;
} IOTypeIndex;

typedef struct SafeAtt
{
    RPY conicalCenRpy;
    double deviation;
} SafeAtt;

typedef struct CatOneStopEffect
{
    /**
     * category_one_stop_program_pause:
     *  运行程序时发生 Cat.1 Stop (e.g 按下急停按钮 安全信号异常)
     *  1: 程序暂停，重新上电使能后可继续运行；
     *  0: 程序停止执行；
    */
    int program_pause;
} CatOneStopEffect;

typedef struct SafePlane_vrep_information
{
    int enable;
    PmCartesian planePoints;
    PmEulerXyz plane_Xyz;
} SafePlane_vrep_information;

typedef struct SafeAttitude_vrep_information
{
    double x_y_cone;
    double z_cone;
    PmEulerXyz tool_dir_xyz;
    PmEulerXyz cone_xyz;
} SafeAttitude_vrep_information;

typedef struct zuc_traj_verp_simulation_safe_information
{
    SafePlane_vrep_information plane[6];
    SafeAttitude_vrep_information tool_cone_dir;
} zuc_traj_verp_simulation_safe_information;

typedef struct
{
    uint8_t program_stop_req;
    uint8_t power_off_req;
    uint8_t servo_disable_req;
    uint8_t program_pause_req;

    // bool stop_mode_req;
    // bool drag_mode_req;		// TIO Key: drag mode toggle request
    // bool drag_mode_exit_req;	// funcdi: exit drag mode request
    // bool drag_mode_entr_req; // funcdi: entr drag mode request
    // bool sys_shutdown;
} Func_IO_Req_t;

typedef struct FuncIOMap
{
    IOTypeIndex task_run_di;
    IOTypeIndex task_pause_di;
    IOTypeIndex task_resume_di;
    IOTypeIndex task_stop_di;
    IOTypeIndex power_on_di;
    IOTypeIndex power_off_di;
    IOTypeIndex servo_enable_di;
    IOTypeIndex servo_disable_di;
    IOTypeIndex back_to_inipos_di;
    IOTypeIndex stop_mode_di;
    IOTypeIndex percentage_mode_level1_di;
    IOTypeIndex percentage_mode_level2_di;
    IOTypeIndex clear_error_di;
    IOTypeIndex drag_mode_di;
    IOTypeIndex drag_mode_disable_di;
    IOTypeIndex task_idle_do[4];  //
    IOTypeIndex task_paused_do[4];
    IOTypeIndex task_running_do[4];
    IOTypeIndex stat_fault_do[4];
    IOTypeIndex stat_poweron_do[4];
    IOTypeIndex stat_enabled_do[4];
    IOTypeIndex stat_motion_do[4];
    IOTypeIndex stat_static_do[4];
    IOTypeIndex stat_boot_do[4];
    IOTypeIndex stat_emergency_stop_do[4];
    IOTypeIndex stat_percentage_mode_level2_do[4];
    IOTypeIndex stat_sys_protective_stop_do[4];
    IOTypeIndex stat_initial_pose_do[4];
    IOTypeIndex stat_percentage_mode_level1_do[4];
    IOTypeIndex stat_drag_mode_do[4];
    IOTypeIndex stat_collision_mode_do[4];
    IOTypeIndex stat_manual_mode_do[4];
    IOTypeIndex stat_auto_mode_do[4];
    fd_set digtalOutput_bound[7];  //用于标记哪些DO 被绑定到了功能IO 或 安全IO 。
                                   // 0: 控制柜IO, 1: Tio  2: 扩展IO，   3:弃用 ，4：modbus IO， 5：profinet IO，  6：EIP  IO
} FuncIOMap;

typedef enum
{
    FUNCDI_RSV = 0,
    FUNCDI_NONE = 1,
    FUNCDI_EX_ESTOP = 2,
    FUNCDI_SAFETYGUARD = 3,
    FUNCDI_RESET_SAFETYGUARD = 4,
    FUNCDI_REDUCE_MODE = 5,
    FUNCDI_THREE_POSITION_ENABLING = 6,  //Three position enabling
    SAFETYDI_COLLISION_LEVEL0 = 7,
    SAFETYDI_COLLISION_LEVEL1 = 8,
    SAFETYDI_COLLISION_LEVEL2 = 9,
    SAFETYDI_COLLISION_LEVEL3 = 10,
    SAFETYDI_COLLISION_LEVEL4 = 11,
    SAFETYDI_COLLISION_LEVEL5 = 12,
    SAFETYDI_COLLISION_LEVEL7 = 13,
    // FUNCDI_SWITCH_MODE = 7
} SafetyDIFunc;  // keep in line with scb and app

//safety DI Settingss
typedef struct SafetyFuncDIMap
{
    short diChnFunc[8];  // SafetyDIFunc
    short drag_limit;    //安全DI三位置使能是否影响拖拽模式的配置
} SafetyFuncDIMap;

typedef enum
{
    FUNCDO_RSV = 0,
    FUNCDO_NONE = 1,
    FUNCDO_BTN_ESTOP = 2,
    FUNCDO_SYS_ESTOP = 3,
    FUNCDO_SAFETYGUARD = 4,
    FUNCDO_MOVING = 5,
    FUNCDO_NOTSTOPPING = 6,
    FUNCDO_REDUCED_MODE = 7,
    FUNCDO_NOTREDUCEMODE = 8,
    SAFETYDO_COLLISION_LEVEL0 = 9,
    SAFETYDO_COLLISION_LEVEL1 = 10,
    SAFETYDO_COLLISION_LEVEL2 = 11,
    SAFETYDO_COLLISION_LEVEL3 = 12,
    SAFETYDO_COLLISION_LEVEL4 = 13,
    SAFETYDO_COLLISION_LEVEL5 = 14,
    SAFETYDO_COLLISION_LEVEL7 = 15,
} SafetyDOFunc;  // keep in line with scb and app

//safety DO Settingss
typedef struct SafetyFuncDOMap
{
    short doChnFunc[8];  // SafetyDOFunc
} SafetyFuncDOMap;

typedef struct SafetyDIStatus
{
    unsigned char add_estop;
    unsigned char add_sfgsafetyguard_stop;
    unsigned char reduce_mode;
    unsigned char sfgsafetyguard_reset;
    unsigned char three_position_enabling;
} SafetyDIStatus;
typedef struct SafetyDOStatus
{
    //当控制柜上的紧急停止按钮被按下时，这些输出对变成低电平。注意使用外部紧急停止按钮的紧急停止和附加紧急停止输入对这些输出没有影响。
    unsigned char button_estop;
    // 当机器人进入Estop状态时，这些输出对变成低电平。控制柜上的Estop按钮、外部Estop按钮的紧急停止以及额外的紧急停止输入都会对这些输出产生影响。
    unsigned char system_estop;
    unsigned char system_safeguard;
    unsigned char robot_moving;
    unsigned char not_stopping;
    unsigned char reduce_status;
    unsigned char not_reduce;

} SafetyDOStatus;

struct ServoParam
{
    enum
    {
        CLSN_SENSITIVITY = 1,
        BRAKE_CRTL_PARAM = 2,
        MULTI_TURN_STEP = 3,
        TORQUE_FORWARD_ENA = 4,
        GENERIC_CMD = 0xFFFF
    } paramType;
    union ServpParamVal
    {
        int clsn_sensitivity;
        int torque_forward_ena;
        struct BrakeCtrl
        {
            int jointId;
            int releaseBrake;  // 1: release; 0: reset
        } brakeCmd;
        struct MultiTurnStep
        {
            int jointId;
            int multiTurnStep;
        } multiTurnCmd;
        struct GeneralCmd
        {
            int jointId;
            int paramId;
            int value;
        } geneCmd;
    } paramVal;
};

typedef struct ServoMoveFilterParam
{
    int filterType;
    int mmf_buf_size;
    float mmf_ka;
    float mmf_kv;
    float mmf_kp;
    float nlf_vp;
    float nlf_ap;
    float nlf_jp;
    float nlf_vr;
    float nlf_ar;
    float nlf_jr;
    float lpf_cutoff_freq;
} ServoMoveFilterParam;

typedef struct FtConfig
{
    unsigned char enableFt;
    unsigned char enableNTrack;
    float ftUser;
    float ftReboundFK;
    float ftConstant;
} FtConfig;

typedef struct EndForceCond
{
    unsigned char enableEndCond;
    int lowFlag;
    int upFlag;
    double lowLimitForce;
    double upLimitForce;
} EndForceCond;

typedef struct TorqueControlInterface
{
    int command_to_close;
    int torque_last_period;
    int torque_last_count;
    int torque_control_enable;
    double current_torque_val[ZUCMOT_MAX_JOINTS];  // 当前周期的力矩指令
} TorqueControlInterface;

typedef struct TorqueControlVal
{
    double torque[ZUCMOT_MAX_JOINTS];
} TorqueControlVal;

#define MAX_TORQUE_CONTROL_QUE_LEN 100

typedef struct TorqueControlQue
{
    int front;
    int rear;
    TorqueControlVal torque_que[MAX_TORQUE_CONTROL_QUE_LEN];
} TorqueControlQue;

/*data structure for IO module extension*/

#define MAX_EXTIO_CNT 8
#define MAX_EXTIO_DI_NUM 64
#define MAX_EXTIO_DO_NUM 64
#define MAX_EXTIO_AI_NUM 32
#define MAX_EXTIO_AO_NUM 32
// #define MAX_EXTIO_NAME_LEN 129  // '\0'

//
typedef struct NetComm
{
    char ipaddr[16];  ///< ip addr of the Modbus-tcp IO module, like "192.168.1.20"
    int port;         ///< port number of the tcp
    int slaveId;
} NetComm;

typedef struct ModRtuComm
{
    int slaveId;   ///< slave id
    int baudrate;  ///< baudrate of the modbus
    int databits;  ///< data length  7/8
    int stopbits;  ///< stop bits  1/2
    int parity;    ///< checking bits 78-> N 79->O 69->E
} ModRtuComm;
//
typedef union CommInfo
{
    NetComm tcpComm;
    ModRtuComm rtuComm;
} CommInfo;

typedef struct PinSetup
{
    int startRegAddr;  ///< start register address of the pin with the same type (di, do, ai, ao)
    int pinNum;        ///< pin number with the same type
} PinSetup;
//
typedef struct ExtIOSetup
{
    int extType;                       ///< 0: Modbus-RTU, 1: Modbus-TCP
    char modName[MAX_EXTIO_NAME_LEN];  ///< name of IO module
    CommInfo commInfo;                 ///< communication settup, union of tcp and rtu
    PinSetup diSetup;                  ///< setup for digital inputs
    PinSetup doSetup;                  ///< setup for digital outputs
    PinSetup aiSetup;                  ///< setup for analog inputs
    PinSetup aoSetup;                  ///< setup for analog outputs
    char ExtIOModInfo[MAX_EXTIO_NAME_LEN];
} ExtIOSetup;

typedef struct ExtIOMap
{
    int diStartIdx;  ///< assigned starting index in the mapped buffer for digital inputs of this module
    int doStartIdx;  ///< assigned starting index in the mapped buffer for digital outputs of this module
    int aiStartIdx;  ///< assigned starting index in the mapped buffer for analog inputs of this module
    int aoStartIdx;  ///< assigned starting index in the mapped buffer for analog outputs of this module
} ExtIOMap;

typedef struct ExtIOModConfig
{
    ExtIOSetup extIOSetup;  ///< setup from customers
    ExtIOMap extIOMap;      ///< pin map from controller
} ExtIOModConfig;

typedef struct ExtIOStat
{
    int status;                      ///< status of extio module: 0(maintenance), 1(operation), 2(fault)
    int errCode;                     ///< error code of the extio modules
    int extDI[MAX_EXTIO_DI_NUM];     ///< status of extended digital input
    int extDO[MAX_EXTIO_DO_NUM];     ///< status of extended digital output
    double extAI[MAX_EXTIO_AI_NUM];  ///< status of extended analog input
    double extAO[MAX_EXTIO_AO_NUM];  ///< status of extended analog output
} ExtIOStat;

typedef struct ExtIOCtrl
{
    int extDO[MAX_EXTIO_DO_NUM];     ///< expected status of extended digital output
    double extAO[MAX_EXTIO_AO_NUM];  ///< expected value of extended analog output
} ExtIOCtrl;

typedef struct ExtIOShm
{
    int mode;     ///< mode_maintenance(0)/mode_operation(1)
    int cabType;  ///< cabinet type: 1 for cab v1.0, 2 for cab v2.1
    int extIOModCount;
    int reConnectInterval;
    int reConnectMaxTimes;
    ExtIOModConfig extIOModConfig[MAX_EXTIO_CNT];  ///< data buffer for extended IO module setup
    ExtIOStat extIOStat;                           ///< data buffer for extended IO status
    ExtIOCtrl extIOCtrl;                           ///< data buffer for extended IO control
} ExtIOShm;

typedef struct MbRtuCommPara_s
{
    int Baudrate;
    int ByteLen;
    char Parity;
    int StopBitLen;
} MbRtuCommPara_t;

typedef struct MbTcpCommPara_s
{
    char IpAddr[16];
    int Port;
} MbTcpCommPara_t;

typedef struct MbSlaveCommPara_s
{
    int MbCommType;
    int SlaveId;
    MbRtuCommPara_t RtuCommPara;
    MbTcpCommPara_t TcpCommPara;

    int IndTimeout_sec;
    int IndTimeout_usec;
    int RespTimeout_sec;
    int RespTimeout_usec;
} MbSlaveCommPara_t;

typedef struct MbSlaveShm
{
    MbSlaveCommPara_t MbSlaveCommPara;
    int zuc_cab_type;
    unsigned int coils[MB_COIL_GEN_ALL];
    unsigned int discrete_input[MB_D_INPT_GEN_ALL];  //
    unsigned int input_reg[MB_IN_REG_ALL];           //
    unsigned int holding_reg[MB_HOLDING_ALL];
} MbSlaveShm_t;

typedef union
{
    unsigned short input_reg_u16[96];
    short input_reg_s16[96];
    float input_reg_f32[48];
} MbSlaveInputReg_u;

typedef union
{
    unsigned short input_reg_u16[2];
    float input_reg_f32;
} Robotstate_u;

typedef struct ModbusIOShm
{
    ExtIOShm extIOShmStruct;
    MbSlaveShm_t MbSlaveShmStruct;
} ModbusIOShm_t;

typedef struct PhysicCabAO
{
    uint8_t data_1_h;
    uint8_t data_1_l;
    uint8_t data_2_h;
    uint8_t data_2_l;
} PhysicCabAO_t;

typedef union
{
    PhysicCabAO_t scb_ao_ctl;
    uint16_t data[2];  //union 中  PhysicCabAO_t 以及SCB CAN 协议定义的字节序为大端模式，系统字节序目前大部分为小端模式，操作cab AIO时需小心

} PhysicCabAO_u;

typedef struct PhysicIOShm
{
    uint8_t physic_do[18];
    PhysicCabAO_u physic_cab_ao;  //only cab2.0 has ao
    uint8_t safety_do[8];         //only cab2.0 has safety DO,每对DO为一个通道
} PhysicIOShm_t;

typedef union
{
    short holding_reg_s16[96];
    unsigned short holding_reg_u16[96];
    float holding_reg_f32[48];
} ModbusIoHoldingReg_u;

typedef struct TorqueControlShm
{
    TorqueControlQue torque_que;
    int enable;
} TorqueControlShm;

typedef struct AutoWorkConfig
{
    int enable;
    char filename[256];
} AutoWorkConfig;

typedef struct ScriptsBackupSetting
{
    int enable;
    float interval;
} ScriptsBackupSetting;

typedef struct TIOInfo
{
    int diNum;  ///< di number of tio
    int doNum;  ///< do number of tio
    int aiNum;  ///< ai number of tio
    int aoNum;  ///< ao number of tio
} TIOInfo;

typedef struct DynParam
{
    float kineticK[ZUCMOT_MAX_JOINTS * 14];
    float dynaticK[ZUCMOT_MAX_JOINTS * 10 + CART_DIM + CART_DIM];
} DynParam;

typedef struct DragControlPara
{
    int drag_control_filter_factor[ZUCMOT_MAX_JOINTS];
    int drag_control_fv_compensate[ZUCMOT_MAX_JOINTS];
    int drag_control_fc_compensate[ZUCMOT_MAX_JOINTS];
    int drag_control_enable;
    double drag_control_dither_factor[ZUCMOT_MAX_JOINTS];
} DragControlPara;

typedef struct RobotModelData
{
    int model;
    double payload;
    DHParam nominalDH;
    DynParam dynParam;
    double maxTrajVel;
    double maxTrajAcc;
    double clsnTrajAcc;  //碰撞检测后减速停止减速度
    double maxTrajVel_ori;
    double maxTrajAcc_ori;
    double clsnTrajAcc_ori;
    double maxJointPosLimit[ZUCMOT_MAX_JOINTS];
    double minJointPosLimit[ZUCMOT_MAX_JOINTS];
    double maxJointVel[ZUCMOT_MAX_JOINTS];
    double maxJointAcc[ZUCMOT_MAX_JOINTS];
    double joint_err_limit[ZUCMOT_MAX_JOINTS];
    double motorRatedCurr[ZUCMOT_MAX_JOINTS];
    double motorRatedTorq[ZUCMOT_MAX_JOINTS];
    double motorRatedTransRatio[ZUCMOT_MAX_JOINTS];
    int motorSupplierId[ZUCMOT_MAX_JOINTS];
    double defShrinkenPose[ZUCMOT_MAX_JOINTS];
    double defStretchedPose[ZUCMOT_MAX_JOINTS];
    double defInitalJPos[ZUCMOT_MAX_JOINTS];
    float gearboxMaxTorque[ZUCMOT_MAX_JOINTS];
    double clsnJointAcc[ZUCMOT_MAX_JOINTS];
    TIOInfo tioInfo;
    double minMomentum;
    double maxMomentum;
    double minRobotPower;
    double maxRobotPower;
    double minReduceMomentum;
    double maxReduceMomentum;
    double minReduceRobotPower;
    double maxReduceRobotPower;
    double robotPowerLimit[2];
    double robotMomentumLimit[2];
    double robotEndforceLimit[2];
    double robotVelocityLimit;
    double SafezoneElbowRadius;
    double SafezoneElbowOffset;
    double dynamics_identify_traj_para[4 * 7 * ZUCMOT_ROBOT_MAX_JOINTS];
    double joint_offset[ZUCMOT_MAX_JOINTS];
    int joint_dir[ZUCMOT_MAX_JOINTS];
} RobotModelData;

typedef struct JointMonitorData
{
    float instCurrent;
    float instVoltage;
    float instTemperature;
    float insAveragePower;  //瞬时平均功率
    double currentFluctuation;
    double sumRunningCircles;
    double sumRunningTime;
    double curRunningCircles;
    double curRunningTime;
    float positionDiff;      // 通道1  ch6-位置误差
    float dualEncoderDiff;   // 通道2  ch15-双编码器差值
    float jointDcVol;        // 通道3  ch90-关节母线电压
    float joint_tor_sensor;  // Atom for torqur sensor
    float joint_ext_tor;     // Atom 关节外力矩
    float reserved;          // 通道4  预留
    double jointTorque;
    double jointTorqueCmd;
} JointMonitorData;

typedef struct
{
    float startJntPos[ZUCMOT_MAX_JOINTS];
    float endJntPos[ZUCMOT_MAX_JOINTS];
} JointTraj;

typedef struct
{
    int mode;  // 0: Modbus RTU, 1: RS485, 2: torque sensor
    ModRtuComm rtuComm;
} TIORSChnConfig;

typedef struct
{
    int diCfg;  // 0:x00 DI2 NPN,DI1 NPN; 1:0x01 DI2 NPN,DI1 PNP; 2:0x10 DI2 PNP,DI1 NPN; 3:0x11 DI2 PNP,DI1 PNP
    int doCfg;  // 高4位为DO2配置，低四位为DO1配置, 0x0 NPN输出; 0x1 PNP输出; 0x2 推挽输出; 0xF RS485H接口使能，DO禁止
    int aiCfg;  // 0:模拟输入功能使能，RS485L禁止; 1:RS485L接口使能，模拟输入功能禁止
} TIOPinConfig;

typedef struct
{
    TIOPinConfig pinCfg;         // settings for all tio pins
    TIORSChnConfig rsChnCfg[2];  // settings for 2 rs485 channels
    int voutEna;                 // TIO电源输出使能, 1:VOUT输出使能, other: disable
    int voutVol;                 // TIO输出电压值, 0:TIO输出电压值为24V, 1:TIO输出电压值为12V
} TIOConfig;

typedef struct
{  // user high-speed interface
    short dir;
    long pulses;
    long speed;
} UhiData;

typedef struct
{
    unsigned char ioType;  // 0: cabinet IO, 1: tool IO, 2: ext IO
    unsigned char ioIndex;
    unsigned char trigEvent;  // 0: falling edge, 1: rising edge, 2: low level, 3: high level
} IOTrigger;

typedef union
{
    ZucPose cartePose;
    double jointPos[ZUCMOT_MAX_JOINTS];
} StateUnion;

typedef struct
{
    int stateType;      // 0: joint position, 1: cartesian position
    IOTrigger trigger;  // the trigger of  the capture
} CaptureSettings;

typedef struct
{
    int captStatus;
    int stateType;  // 0: joint position, 1: cartesian position
    StateUnion captRes;
} CaptureStatus;

typedef struct
{
    PmHomogeneous T1_to_world;  //第一关节坐标在世界坐标系下的齐次矩阵
    PmHomogeneous T2_to_world;
    PmHomogeneous T3_to_world;
    PmHomogeneous T4_to_world;
    PmHomogeneous T5_to_world;
    PmHomogeneous T6_to_world;
    PmHomogeneous T7_to_world;
} MultiPmHomogeneous;

// @FIXME liwang 7轴雅可比矩阵
typedef struct JacobianMat
{
    double jacob[6][6];
    double jacob_7[6][7];
    double jacob_7_7[7][7];
} JacobianMat;
typedef struct
{
    int joint_id;
    double positive_limit;
    double negative_limit;
} JointPosLimit;

typedef struct
{
    int checkSafezone;
    int pre_checkSafezone;
} checksafefalg_out;

typedef struct JointConfig_t
{
    int Inited;
    unsigned char Type;  // non-zero means joint called init
    double Units;
    double MaxVel;
    double MaxAccel;
    double MinLimit;
    double MaxLimit;
    double ErrLimit;
    double HMaxVel;  // hard maximum velocity, in ZUXXX.ini
} JointConfig_t;

typedef struct AxisConfig_t
{
    int Inited;
    unsigned char Type;
    double MaxVel;
    double MaxAccel;
    double Home;
    double MinLimit;
    double MaxLimit;
} AxisConfig_t;

typedef struct TrajConfig_t
{
    int Inited;  // non-zero means traj called init
    int Joints;
    double MaxAccel;
    double MaxVel;
    double MaxJointJerk;
    double MaxCarteJerk;
    int DeprecatedAxes;
    int AxisMask;
    double LinearUnits;
    double AngularUnits;
    int MotionId;
    //added by alex.li aat 2018.08.03
    double MaxJerk;
    double baseRx;
    double baseRy;
    double baseRz;
    ZucPose toolOffset[16];
    ZucPose userOffset[16];
    int safeZoneOpenEnable;
    int safeZoneAutoEnable;
    double SafezoneElbowRadius;
    double SafezoneElbowOffest;
    int filterType;
    int mmf_buf_size;
    double mmf_ka;
    double mmf_kv;
    double mmf_kp;
    double nlf_vp;
    double nlf_ap;
    double nlf_jp;
    double nlf_vr;
    double nlf_ar;
    double nlf_jr;
    double lpf_cutoff_freq;
} TrajConfig_t;

#define ZERO_ZUC_POSE(pos) \
    do {                   \
        pos.tran.x = 0.0;  \
        pos.tran.y = 0.0;  \
        pos.tran.z = 0.0;  \
        pos.a = 0.0;       \
        pos.b = 0.0;       \
        pos.c = 0.0;       \
        pos.u = 0.0;       \
        pos.v = 0.0;       \
        pos.w = 0.0;       \
    } while (0)
#endif
